close all;
clear;
clc;

%% 1
A = [0 1; 0 0]
B = [0; 1]
cT = [1 0]
d = [0]

A = [3 3; 2 0]
B = [0 3; 3 -1]
cT = [1 -2]
d = [0 0]

syms Ta

% Euler
Phi1 = eye(2) + A*Ta
Gamma1 = B
% exakt
Phi2 = expm(A*Ta)
%Phi2 = eye(2) + A*Ta + A^2*Ta^2/2
Gamma2 = int(Phi2, Ta, 0, Ta)*B

Ta = 1
Phi1subbed = subs(Phi1)
Gamma1subbed = subs(Gamma1)
Phi2subbed = subs(Phi2)
Gamma2subbed = subs(Gamma2)

%% 2
Phi3 = [-14 -90 7 -56; 10 84 -10 55; 0 -18 -7 -12; -14 -126 14 -83]
Gamma3 = [3; -2; 1; 3]

R = [Gamma3, Phi3*Gamma3, Phi3^2*Gamma3, Phi3^3*Gamma3, Phi3^4*Gamma3, Phi3^5*Gamma3]
rank(R)
Rred = R(:,1:rank(R))

% x = l1 * v1 + l2 * v2

% e^T_n = Gamma^T_R = v^T_1 * R
PhiS = [0 1; -6 -7]
GammaS = [0; 1]
poles = roots([1 -3/10 1/50])
-acker(PhiS, GammaS, poles)
%% 3
Phi4 = [9/8 -21/8 7/2; 13/8 7/8 1/2; -3/8 27/8 -13/4]
Gamma4 = [5; -1; -5]
cT2 = [1 -1 2]
Phi4g = [0 1 0; 0 0 1; 0 0 0]
syms k1 k2 k3
kT = [k1 k2 k3]
Gamma4 * kT
Phi4g - Phi4
% Phi4 + Gamma4 * kT = Phi4g

kT = -acker(Phi4', cT2', [0 0 0])

e = [1;1;1];
for i=0:3000
    e = Phi4*e;
end
norm(e)